#include "SpringNet.h"
#include "Renderer.h"
#include "PhysicsNode.h"

int SpringNet::initPos = 2;
SpringNet::SpringNet() {
	numBalls = 20;
	mass = 100.0f;
	ks = 0.1f;
	kd = 1.f;
	initScale = true;
	for (int i = 0; i < numBalls; i++)
	{
		for (int j = 0; j < numBalls+i; j++)
		{
			positions.push_back(Vector3(0, 0, 0));
			localPos.push_back(Vector3(0, 0, 0));
			origPos.push_back(Vector3(0, 0, 0));
			velocities.push_back(Vector3(0, 0, 0));
			forces.push_back(Vector3(0, 0, 0));
		}
	}

	for (int i = 0; i < numBalls; i++)
	{
		for (int j = 0; j < numBalls; j++)
		{	
			localPos[i * numBalls + j] = Vector3((1.4f / (numBalls - 1) + i / 100.f) * j - i / 10.f - 0.7f, -1.4f / (numBalls - 1) * i+1.5f, -0.1f * i/5.f-0.3f);
		}
	}
}

void SpringNet::Update(float msec) 
{
	for (int i = 0; i < numBalls; i++) {
		for (int j = 0; j < numBalls; j++)
		{
			Vector3 f0 = (origPos[i*numBalls + j] - positions[i*numBalls + j])*ks + Vector3(0, 0, -0.03*(rand() % 499 + 1) / 10.f);
			forces[i*numBalls + j] = f0 - velocities[i*numBalls + j] * kd + PhysicsNode::gravity*mass/1000.f;
		}
	}
	float invMass = 1.0f / mass;
	for (int i = 0; i < numBalls; i++) {
		for (int j = 0; j < numBalls; j++)
		{				
			Vector3 a = forces[i*numBalls + j] * invMass;
			velocities[i*numBalls + j] = velocities[i*numBalls + j] + a*msec;
			positions[i*numBalls + j] = positions[i*numBalls + j] + velocities[i*numBalls + j] * msec;
		}
	}
}

void SpringNet::NodeUpdate(float msec)
{
	Matrix4 worldTrans = target->GetWorldTransform();
	Vector3 scale = target->GetModelScale();
	if (initScale)
	{
		spScale = scale;
		initScale = false;
	}
	for (int i = 0; i < numBalls; i++) {
		for (int j = 0; j < numBalls; j++)
		{
			origPos[i * numBalls + j] = (worldTrans * Matrix4::Scale(spScale) * Vector4(localPos[i * numBalls + j])).ToVector3();
			if (initPos>0)
			{
				positions[i * numBalls + j] = origPos[i * numBalls + j];
				velocities[i*numBalls + j] = Vector3();
			}
			else
			{
				positions[i*numBalls + j] = origPos[i*numBalls + j] + (positions[i*numBalls + j] - origPos[i*numBalls + j]) / (numBalls - 1)*i;
			}
		}
	}
	if (initPos>0)
	{
		initPos--;
	}
	
	target->SetWorldTransform(Matrix4());
	target->SetModelScale(Vector3(1, 1, 1));
}

float SpringNet::randFloat(int start, int end) {
	float r = rand()*1.0 / RAND_MAX;
	return (end - start)*r + start;
}